#ifndef __EXP_NODE_H__
#define __EXP_NODE_H__

// 头文件
#include <ros/ros.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/SetMode.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <std_msgs/Int32.h>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <unsupported/Eigen/KroneckerProduct>
#include <cmath>
#include <iostream>

using namespace std;

extern const int N_drone;        // 无人机数目（包含领导者）
extern const int N_UGV;          // 无人车数目
extern const double H_flight_plain; // 无人机飞行高度（固定）
extern const int N_obs;          // 障碍物数目

// 领导者部分参数 状态量为px,vx,py,vy
// 轨迹参数
extern double r_leader;  // 运动范围
extern double omega_leader;  // 运动速度
extern double alpha_p;
extern double alpha_v;     // 取零使其振荡运动
// 状态矩阵
extern Eigen::Matrix4d S;
extern Eigen::MatrixXd F;

// 跟随者部分参数
// 无人机UAV
extern double alpha_p_uav;
extern double alpha_v_uav;
// 状态矩阵
extern Eigen::Matrix4d A_uav;
extern Eigen::MatrixXd B_uav;
extern Eigen::MatrixXd C_uav;
extern Eigen::Matrix4d X_uav;   
extern Eigen::MatrixXd U_uav;
extern Eigen::MatrixXd K1_uav;  // 这里后续需要调参，预设部分就是自动生成了                
extern Eigen::MatrixXd K2_uav;

// 无人车UGV
// 状态矩阵
extern Eigen::Matrix2d A_ugv;
extern Eigen::Matrix2d B_ugv;
extern Eigen::Matrix2d C_ugv;
extern Eigen::MatrixXd X_ugv;
extern Eigen::MatrixXd U_ugv;
extern Eigen::MatrixXd K1_ugv;  // 这里后续需要调参，预设部分就是自动生成了                
extern Eigen::MatrixXd K2_ugv;

// 初始值
// 初始观测值 每一列是一个智能体，从上至下为px vx py vy
extern Eigen::Matrix4d init_obrv;
extern Eigen::Matrix4d obrv_esti;

// 障碍物位置
extern Eigen::MatrixXd obs_pos;

// 编队向量类型
typedef struct{
    double px;
    double vx;
    double ax;
    double py;
    double vy;
    double ay;
}h_t;

h_t get_funh(int k, double t);

#endif